/*
 * helpers.cpp
 *
 *  Created on: Sep 18, 2011
 *      Author: andrew
 */

#include "helpers.h"
#include "cv.h"
#include "highgui.h"
using namespace cv;
using namespace std;

String getRelativePath(String programPath)
{
	String relativePath = programPath.substr(0, programPath.rfind('/'));
	return relativePath;
}

void loadImages(String progPath, String rightImageName, String leftImageName, Mat* imgRight, Mat* imgLeft)
{
	String imageFolder = "/images";
	String relativePath = getRelativePath(progPath);
	String imagePath = relativePath + imageFolder;
	String leftImagePath = imagePath + "/" + leftImageName;
	String rightImagePath = imagePath + "/" + rightImageName;
	//cout << leftImagePath << endl << rightImagePath << endl;

	*imgLeft = imread(leftImagePath);
	*imgRight = imread(rightImagePath);

	return;
}

Mat ssdStereoMatch(Mat imr, Mat iml)
{
	Mat a;
	return a;
}

Point2i bestMatch(Mat templ, Mat image)
{
	float lowestMatch = 1.00e300;
	int rows, cols;
	int templRows, templCols;
	templRows = templ.rows;
	templCols = templ.cols;
	rows = image.rows;
	cols = image.cols;
	Point2i lowestMatchPoint;
	for (int j = 0 + templRows; j < rows - templRows; ++j)
	{
		for (int i = 0 + templCols; i < cols - templCols; ++i)
		{
			float val = ssdValue(templ, image, j, i);
			if(val < lowestMatch)
			{
				lowestMatch = val;
				if(j < 20)
					cout << "new lowest: " << val << endl;
				lowestMatchPoint.x = i;
				lowestMatchPoint.y = j;
			}
		}
	}
	return lowestMatchPoint;
}

int ssdValue(Mat templ, Mat image, int r, int c)
{
	//Image is assumed to have one channel and be floating point
	float sum = 0;
	int rows, cols;
	rows = templ.rows;
	cols = templ.cols;
	for (int j = 0; j < rows; ++j)
	{
		for (int i = 0; i < cols; ++i)
		{
			int coord1 = r + j - (rows / 2);
			int coord2 = c + i - (cols / 2);

			float val = templ.ptr<float>(j)[i] -
						image.ptr<float>(coord1)[coord2];

			sum += (val * val);
		}
	}
	return sum;
}
Mat fromChannel(Mat mat, int channel) //Type should be one channel
{
	Mat grayImage(mat.size(), CV_8UC1);
	for(int i = 0; i < mat.rows; ++i)
	{
		for(int j = 0; j < mat.cols; ++j)
		{
			grayImage.ptr<unsigned char>(i)[j] = mat.ptr<unsigned char>(i)[3*j + channel];
		}
	}
	return grayImage;
}

void scaleVals(cv::Mat mat, float scaleFactor)
{
	for(int i = 0; i < mat.rows; ++i)
	{
		for(int j = 0; j < mat.cols; ++j)
		{
			mat.ptr<float>(i)[j] = mat.ptr<float>(i)[j] * scaleFactor;
		}
	}
}
